from djitellopy import tello
import time
import cv2
import numpy as np

me = tello.Tello()
me.connect()
print(me.get_battery())

me.takeoff()
time.sleep(2)
me.streamon()

def find_circle(frame):
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
    circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, param1=50, param2=30, minRadius=20, maxRadius=200)

    if circles is not None:
        circles = np.round(circles[0, :]).astype('int')
        return circles
    return []

def find_max_circle(circles):
    if circles:
        max_circle = max(circles, key=lambda x: x[2])
        return max_circle
    return None


while True:
    frame = me.get_frame_read().frame
    # circles = find_circle(frame)
    cv2.imshow("Frame", frame)
    # time.sleep(5)
    # if circles:
    #     max_circle = find_max_circle(circles)
    #     if max_circle:
    #         x, y, r = max_circle
    #         print(f"Found circle at ({x}, {y}), radius:{r}")
    #         cv2.circle(frame, (x, y), r, (0, 0, 255), 4)



    #
    # else:
    #     me.send_rc_control(0, 0, 0, 60)
    #     print("404")

    if cv2.waitKey(1) & 0xFF == ord('q'):
        me.land()
        break